#include "vehicle_linear2d2s.h"
#include "mpc.h"
#include <iostream>
#include <yaml-cpp/yaml.h>
#include <chrono>
#include <string>
using namespace LinearMpc;
namespace auto_ros
{
namespace control
{
class Linear2d2sMpc
{
public:
	Linear2d2sMpc(const std::string dyn_config_filename, const std::string mpc_config_filename, double rate);
	bool step(const Eigen::MatrixXd &q, const Eigen::MatrixXd &r,
			  const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb, const Eigen::MatrixXd &x_ref,
			  const Eigen::MatrixXd &distur_ref, double speed);
	bool step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb, const Eigen::MatrixXd &x_ref,
			  const Eigen::MatrixXd &distur_ref, double speed); //q r weighting factor will regulate auto based on yaml file
	Eigen::MatrixXd base_state_matrix();
	std::shared_ptr<Mpc> vehicle_mpc_ptr_;
	std::shared_ptr<VehicleLinear2d2s> vehicle_dyn_ptr_;
	std::shared_ptr<DiscreteLinearSys> dis_dyn_ptr_;
	YAML::Node mpc_yaml();

protected:
	//YAML::Node vehicle_dyn_config_;
	YAML::Node mpc_yaml_;

	double rate_;
};

} // namespace control
} // namespace auto_ros